/**
 * encoding:UTF-8
 * @file naive_chassis_task.c
 * @authors 电控组 UCAS-SAS 中国科学院大学机器人队
 * @brief 
 * @version 31.14
 * @date 2023-02-15
 * 
 * @copyright Copyright (c) 2023
 * 
 * 功能定义
 * 
 */
// 底盘电机ID和建系
/*      前X正
    2           1
左Y正                  右Y负
    3           4
        后X负
*/
// 编号
/*
1  0

2  3
*/

#include "remote_control.h"
#include "nucCommu.h"
#include "CAN_receive.h"
#include "pid.h"
#include "cmsis_os.h"
#include "robot_total_mode.h"
#include "RobotStructure.h"
#include "arm_math.h"
#include "OLED.h"
#include "naive_gimbal_task.h"
#include "user_lib.h"
#include "detect_task.h"
#include "referee.h"
#include "naive_chassis_task.h"
#include <stdio.h>
#include <stdint.h>


//*****************************debug选项*******************//
// #define POWERLESS_DEBUG_FOR_SAFETY

//*************************按键定义********************//
//按键定义
#define HANDLE_LEFT_LR 2    //遥控器左边摇杆的左右方向  left right
#define HANDLE_LEFT_BF 3    //遥控器左边摇杆的前后方向  back forth
#define HANDLE_RIGHT_LR 0    //遥控器右边摇杆的左右方向  left right
#define HANDLE_RIGHT_BF 1    //遥控器右边摇杆的前后方向  back forth
#define SWITCH_SA 5          //遥控器两档乒乓开关 SA
#define SWITCH_SB 7  
#define SWITCH_SC 6  

//普通车模式    common car 按C或者遥控器右拨杆上方开启，（全能坦克）
//平移控制的坐标系为底盘坐标系
#define COMMON_FORTH            KEY_PRESSED_OFFSET_W    //前进
#define COMMON_BACK             KEY_PRESSED_OFFSET_S    //后退
#define COMMON_LEFT_ROTATE      KEY_PRESSED_OFFSET_Q    //左转
#define COMMON_RIGHT_ROTATE     KEY_PRESSED_OFFSET_E    //右转
#define COMMON_LEFT_MOVE        KEY_PRESSED_OFFSET_A    //左平移
#define COMMON_RIGHT_MOVE       KEY_PRESSED_OFFSET_D    //右平移
//而遥控器左摇杆控制前后平移和左右旋转
#define COMMON_BF_RC            HANDLE_LEFT_BF      //前进,后退
#define COMMON_LR_ROTATE_RC     HANDLE_LEFT_LR      //左转,右转

//小陀螺模式    spinner，就像一个旋转的陀螺v，所以按v进入小陀螺模式。或者遥控器右拨杆在中间进入
//平移控制的坐标系为云台坐标系
#define SPINNER_FORTH            KEY_PRESSED_OFFSET_W    //前进
#define SPINNER_BACK             KEY_PRESSED_OFFSET_S    //后退
#define SPINNER_LEFT_MOVE        KEY_PRESSED_OFFSET_A    //左平移
#define SPINNER_RIGHT_MOVE       KEY_PRESSED_OFFSET_D    //右平移
//遥控器左摇杆控制前后平移和左右平移
#define SPINNER_BF_RC            HANDLE_LEFT_BF      //前进,后退
#define SPINNER_LR_MOVE_RC       HANDLE_LEFT_LR      //左转,右转

///////////////////////////////////////////////////////////////////////

//****************************机器人控制常量***************//
#define KEYBOARD_CONTROL_ROBOT_SPEED_X 1.5f      //键盘wasd控制的速度
#define KEYBOARD_CONTROL_ROBOT_SPEED_Y 1.5f
#define KEYBOARD_CONTROL_ROBOT_SPEED_W 1.5f

 //小陀螺运动旋转速度
#ifdef HERO
#define SPINNER_W   0.8f
#endif

#ifdef INFANTRY
#define SPINNER_W   0.3f
#endif



#define ECD_FULL_ROUND 8192 //一圈的ECD值。实际取值0-8191


//****************************控制量计算系数************//
#define RC_CH_MAX 873 //遥控器通道最大值

#ifdef HERO
#define CHASSIS_VX_RC_SEN -0.001f
#define CHASSIS_VY_RC_SEN 0.001f
#define CHASSIS_WZ_RC_SEN 0.0005f 
#define AUTO_VX_MAX ((CHASSIS_VX_RC_SEN>0?CHASSIS_VX_RC_SEN:-CHASSIS_VX_RC_SEN)*RC_CH_MAX)
#define AUTO_VY_MAX ((CHASSIS_VY_RC_SEN>0?CHASSIS_VY_RC_SEN:-CHASSIS_VY_RC_SEN)*RC_CH_MAX)
#define AUTO_WZ_MAX ((CHASSIS_WZ_RC_SEN>0?CHASSIS_WZ_RC_SEN:-CHASSIS_WZ_RC_SEN)*RC_CH_MAX)

#endif

#ifdef INFANTRY
#define CHASSIS_VX_RC_SEN -0.001f   
#define CHASSIS_VY_RC_SEN 0.001f  
#define CHASSIS_WZ_RC_SEN 0.0005f
#define AUTO_VX_MAX ((CHASSIS_VX_RC_SEN>0?CHASSIS_VX_RC_SEN:-CHASSIS_VX_RC_SEN)*RC_CH_MAX)
#define AUTO_VY_MAX ((CHASSIS_VY_RC_SEN>0?CHASSIS_VY_RC_SEN:-CHASSIS_VY_RC_SEN)*RC_CH_MAX)
#define AUTO_WZ_MAX ((CHASSIS_WZ_RC_SEN>0?CHASSIS_WZ_RC_SEN:-CHASSIS_WZ_RC_SEN)*RC_CH_MAX)
#endif

#define CHASSIS_RC_DEADLINE 10  //低于这个值的摇杆读数一律为0

#define MOTOR_SPEED_TO_CHASSIS_SPEED_VX 0.25f   //电机旋线速度转化为机器人底盘速度。麦轮直径15cm，半径8cm
#define MOTOR_SPEED_TO_CHASSIS_SPEED_VY 0.25f  
#define MOTOR_SPEED_TO_CHASSIS_SPEED_WZ 0.25f  

#define MOTOR_DISTANCE_TO_CENTER 0.27f           //电机到机器人中心的距离 27cm，可以在solidworks装配体中测量
// #define M3508_MOTOR_RPM_TO_VECTOR 0.0083f   //底盘电机转速到线速度（m/s）转化。计算：n*2*π*R/60
#define M3508_MOTOR_RPM_TO_VECTOR 0.0004f        //应该还要除以这里大约20.想一想应该是减速电机，转化为实际的速度还要除以减速比。
                                                //这个值还是
//******************全向轮解算所需常量****************//
#define CHASSIS_OMNIWHELL_OMEGA 3.025f //底盘中心到轮轴中心距离比上轮子半径(mm)
#define CHASSIS_OMNIWHELL_SINALPHA 0.7071f //底盘中心到轮轴中心连线与y轴所成角度sin值
#define CHASSIS_OMNIWHELL_COSALPHA 0.7071f //底盘中心到轮轴中心连线与y轴所成角度cos值
#define CHASSIS_OMNIWHELL_SINTHETA 0.7071f //全向轮前进方向与y轴所成角度sin值
#define CHASSIS_OMNIWHELL_COSTHETA 0.7071f //全向轮前进方向与y轴所成角度cos值

//***************************PID控制参数*****************//
#define MAX_MOTOR_CAN_CURRENT 15000.0f  //底盘3508最大can发送电流值
//底盘电机速度环PID
#ifdef INFANTRY
#define M3505_MOTOR_SPEED_PID_KP 10000.0f
#define M3505_MOTOR_SPEED_PID_KI 0.0f
#define M3505_MOTOR_SPEED_PID_KD 0.0f
#define M3505_MOTOR_SPEED_PID_MAX_OUT MAX_MOTOR_CAN_CURRENT
#define M3505_MOTOR_SPEED_PID_MAX_IOUT 2000.0f
#endif

#ifdef HERO
#define M3505_MOTOR_SPEED_PID_KP 10000.0f
#define M3505_MOTOR_SPEED_PID_KI 0.0f
#define M3505_MOTOR_SPEED_PID_KD 0.0f
#define M3505_MOTOR_SPEED_PID_MAX_OUT MAX_MOTOR_CAN_CURRENT
#define M3505_MOTOR_SPEED_PID_MAX_IOUT 2000.0f
#endif


#define MAX_WHEEL_SPEED 4.0f    //单个底盘电机最大速度
#define NORMAL_MAX_CHASSIS_SPEED_X 2.0f //底盘运动过程最大前进速度
#define NORMAL_MAX_CHASSIS_SPEED_Y 1.5f //底盘运动过程最大平移速度
#define NORMAL_MAX_CHASSIS_SPEED_W 0.9f


//*************************任务相关常量*******************//
// TODO: 调车时重新测量中心ECD，放在此文件中
//预先测量的云台在中心时的yawECD。在机器人重新安装云台后，可能导致ECD和这个不一样
#ifdef INFANTRY
#define INIT_PRE_GOT_MID_ECD 1789
#endif
#ifdef HERO
#define INIT_PRE_GOT_MID_ECD 2612
#endif


#define CHASSIS_TASK_INIT_TIME 500      //电机初始化时间
#define CHASSIS_CONTROL_TIME_MS 5       //每5ms执行一次任务

#define rc_deadband_limit(input, output, dealine)        \
    {                                                    \
        if ((input) > (dealine) || (input) < -(dealine)) \
        {                                                \
            (output) = (input);                          \
        }                                                \
        else                                             \
        {                                                \
            (output) = 0;                                \
        }                                                \
    }

//******************数字滤波所需常量****************// 系数和控制方法来自官方步兵
#define CHASSIS_ACCEL_X_NUM 150.1666f
#define CHASSIS_ACCEL_Y_NUM 150.1666f
#define CHASSIS_ACCEL_W_NUM 150.66f

#define limit_abs(input, output, max_abs)   \
{                                           \
    if(input > 0 && input > max_abs)        \
        output = max_abs;                   \
    else if(input < 0 && input < -max_abs)  \
        output =  - max_abs;                \
    else                                    \
        output = input;                     \
}

//******************功率控制相关常量***********************//   // 2.3
#define FULL_BUFFER         60.0f   // 最大缓冲能量
#define DANGEROUS_BUFFER    5.0f    // 危险缓冲能量，绝对不能让缓冲能量低于这个值

#define CURRENT_TO_DATA     819.2f  // 每A电流对应发送的数据。物理的安培对应的CAN数据
#define VOLTAGE             24.0f   // 供电电压为24V

#define COMMUNICATING_PERIOD_S  0.1f     // 与裁判系统的通讯周期。单位为s

// 用于调试具体功率
// #define TEST_P40
// #define TEST_P60
// #define TEST_P80
// #define TEST_P100
// #define TEST_P45
// #define TEST_P50
// #define TEST_P55

// #define NO_BUFFER   // 2.18，无buffer状态，用于调试抬升电流大小

#define abss(input, output)     \
{                               \
    if ((input) >= 0)             \
        (output) = (input);         \
    else                        \
        (output) = -(input);        \
}

//**************************控制量结构体*******************//

// 坐标系
enum MovingAxis_e{
    MovingAxis_e_GimbalAxis,
    MovingAxis_e_ChassisAxis,
};

//机器人控制量
struct RobotControl_s{
    fp32 vx,vy,w;
    first_order_filter_type_t vx_filter,vy_filter,w_filter;
    enum MovingAxis_e axis;
};

//云台yaw电机相对于底盘角度结构体
struct Angle_s{
    int16_t nowGimbalYawECD;        //目前的ECD
    fp32    gimbalAngleFromChassis; //云台相对于最初的角度，逆时针为正方向。单位弧度。
};

//底盘电机控制
struct MotorControl_s{
    pid_type_def    vpid;
    fp32            presentMotorSpeed;
    fp32            wantedMotorSpeed;
    int16_t         giveCurrent;
};


//************************用于控制的全局变量**********//
static int zeroCurrentMark;    //当离线时，直接发送零电流的时候
static struct Angle_s someAngle;   //用于记录所有的云台、底盘角度相关值
static struct MotorControl_s driveMotor[4];
static const RC_ctrl_t *rc_p;   //遥控器位置指针，需要初始化
static const toSTM32_t *nuc_p;  //NUC数据位置。未来制造全自动机器人时需要
const static fp32 motor_speed_pid[3] = {M3505_MOTOR_SPEED_PID_KP, M3505_MOTOR_SPEED_PID_KI, M3505_MOTOR_SPEED_PID_KD}; //PID 初始化参数
static const enum RobotState_e *robotMode;   //机器人模式
static struct RobotControl_s robotTotalSpeedControl;
static int16_t vx_channel,vy_channel,w_channel,sa_channel,sb_channel,sc_channel; //遥控器deadband limit后输出的值

// 功率控制
fp32 powerNow;     // 2.15，上一次裁判系统读到的底盘总功率
fp32 bufferNow = 60.0f;     // 2.15，当前剩余的缓冲能量
fp32 totalCurrentLimit;     // 2.15，底盘电流上限


//************************各个功能函数*************//
static void initPIDs(void)
{
    int i;
    for(i=0;i<4;i++)
        PID_init(&(driveMotor[i].vpid),PID_POSITION,motor_speed_pid,M3505_MOTOR_SPEED_PID_MAX_OUT,M3505_MOTOR_SPEED_PID_MAX_IOUT);
}

static void initFilters(void)
{
    const static fp32 chassis_x_order_filter[1] = {CHASSIS_ACCEL_X_NUM};
    const static fp32 chassis_y_order_filter[1] = {CHASSIS_ACCEL_Y_NUM};
    const static fp32 chassis_w_order_filter[1] = {CHASSIS_ACCEL_W_NUM};
    
    first_order_filter_init(&(robotTotalSpeedControl.vx_filter), CHASSIS_CONTROL_TIME_MS, chassis_x_order_filter);
    first_order_filter_init(&(robotTotalSpeedControl.vy_filter), CHASSIS_CONTROL_TIME_MS, chassis_y_order_filter);
    first_order_filter_init(&(robotTotalSpeedControl.w_filter), CHASSIS_CONTROL_TIME_MS, chassis_w_order_filter);

}

static void setChassisMode(void)
{
    zeroCurrentMark=0;
    if(RobotState_e_Powerless== *robotMode)
        zeroCurrentMark=1;
    else if(RobotState_e_Brilliant == *robotMode)
    {
        #ifdef GAME
        if(game_not_started())
            zeroCurrentMark = 1;
        #endif
    }
}
int16_t*VX_CHANNEL;
int16_t*VY_CHANNEL;
int16_t*W_CHANNEL;
int16_t*SA_CHANNEL;
int16_t*SB_CHANNEL;
int16_t*SC_CHANNEL;
static void analyseTotalControl(void)    //通过遥控器和键盘参数设置速度。当键盘输出量为0时，通过遥控器摇杆确定速度。
{
    // int keyBoardMove=0;
    //注意建系的方向
    robotTotalSpeedControl.axis=MovingAxis_e_GimbalAxis;    //默认云台坐标系，有助于操作手的控制
    robotTotalSpeedControl.vx=0;robotTotalSpeedControl.vy=0;robotTotalSpeedControl.w=0;
    if(RobotState_e_Powerless==*robotMode)
    {
        return;
    }
    else if(RobotState_e_CommonCar==*robotMode)
    {
         robotTotalSpeedControl.axis=MovingAxis_e_GimbalAxis; 
        if(rc_p->key.v & COMMON_FORTH)  //前
            robotTotalSpeedControl.vx+=KEYBOARD_CONTROL_ROBOT_SPEED_X;
        if(rc_p->key.v & COMMON_BACK)  //后
            robotTotalSpeedControl.vx-=KEYBOARD_CONTROL_ROBOT_SPEED_X;                                
        if(rc_p->key.v & COMMON_LEFT_MOVE)  //左
            robotTotalSpeedControl.vy+=KEYBOARD_CONTROL_ROBOT_SPEED_Y;
        if(rc_p->key.v & COMMON_RIGHT_MOVE)  //右
            robotTotalSpeedControl.vy-=KEYBOARD_CONTROL_ROBOT_SPEED_Y;
        if(rc_p->key.v & COMMON_LEFT_ROTATE)    //左旋 从上向下看为逆时针
            robotTotalSpeedControl.w+=KEYBOARD_CONTROL_ROBOT_SPEED_W;
        if(rc_p->key.v & COMMON_RIGHT_ROTATE)   //右旋
            robotTotalSpeedControl.w-=KEYBOARD_CONTROL_ROBOT_SPEED_W;

        rc_deadband_limit(rc_p->rc.ch[COMMON_BF_RC],vx_channel,CHASSIS_RC_DEADLINE);
        robotTotalSpeedControl.vx +=vx_channel*CHASSIS_VX_RC_SEN    ;
        rc_deadband_limit(rc_p->rc.ch[COMMON_LR_ROTATE_RC],w_channel,CHASSIS_RC_DEADLINE);
        rc_deadband_limit(rc_p->rc.ch[SPINNER_LR_MOVE_RC],vy_channel,CHASSIS_RC_DEADLINE);
        if(switch_is_up(rc_p->rc.ch[SWITCH_SA]))//乒乓开关SA朝上时，进入平移模式，拨动左侧轮盘的左右摇杆，可以实现平移
        {
            robotTotalSpeedControl.vy += vy_channel*CHASSIS_VY_RC_SEN;
        }else{
            robotTotalSpeedControl.w +=w_channel*CHASSIS_WZ_RC_SEN;
        }
    }
    else if(RobotState_e_Brilliant==*robotMode)
    {
        robotTotalSpeedControl.axis=MovingAxis_e_GimbalAxis;//云台坐标系
        //设定底盘速度
        if(!toe_is_error(MINI_PC_TOE))
        {
            robotTotalSpeedControl.w=(fp32)nuc_p->w*AUTO_WZ_MAX/(fp32)INT16_MAX;
            robotTotalSpeedControl.vy=-(fp32)nuc_p->x*AUTO_VX_MAX/(fp32)INT16_MAX;
            robotTotalSpeedControl.vx=(fp32)nuc_p->y*AUTO_VY_MAX/(fp32)INT16_MAX;
        }
        else
        {
            robotTotalSpeedControl.vx = 0;
            robotTotalSpeedControl.vy = 0;
            robotTotalSpeedControl.w = 0;
        }
    }
    else if(RobotState_e_Spinner==*robotMode)
    {
         robotTotalSpeedControl.axis=MovingAxis_e_GimbalAxis; //云台坐标系
        if(rc_p->key.v & SPINNER_FORTH)  //前
            robotTotalSpeedControl.vx+=KEYBOARD_CONTROL_ROBOT_SPEED_X;
        if(rc_p->key.v & SPINNER_BACK)  //后
            robotTotalSpeedControl.vx-=KEYBOARD_CONTROL_ROBOT_SPEED_X;
        if(rc_p->key.v & SPINNER_LEFT_MOVE)  //左
            robotTotalSpeedControl.vy+=KEYBOARD_CONTROL_ROBOT_SPEED_Y;
        if(rc_p->key.v & SPINNER_RIGHT_MOVE)  //右
            robotTotalSpeedControl.vy-=KEYBOARD_CONTROL_ROBOT_SPEED_Y;
        rc_deadband_limit(rc_p->rc.ch[SPINNER_BF_RC],vx_channel,CHASSIS_RC_DEADLINE);
        rc_deadband_limit(rc_p->rc.ch[SPINNER_LR_MOVE_RC],vy_channel,CHASSIS_RC_DEADLINE);
        robotTotalSpeedControl.vx += vx_channel*CHASSIS_VX_RC_SEN;
        robotTotalSpeedControl.vy += vy_channel*CHASSIS_VY_RC_SEN;
        //内置旋转速度控制程序
        //最简单的，单级spinner 速度
        // 从云台控制那里获取云台相对于底盘转过的圈数，云台逆时针转为正圈数。若到达顶峰，则让底盘也逆时针转动
        robotTotalSpeedControl.w=SPINNER_W;
    }    
}

static void refreshECD(void)   //更新当前云台yawECD值和弧度制的值，实现云台坐标系控制以及小陀螺
{
    someAngle.nowGimbalYawECD=0;
    someAngle.gimbalAngleFromChassis=(someAngle.nowGimbalYawECD - INIT_PRE_GOT_MID_ECD) * 2 * PI / ECD_FULL_ROUND;
}

static void calcWheelVelocityInChassisAxis(void)   //将控制量中的坐标系速度转化为底盘坐标系的速度，并计算轮子转动速度
{
    fp32 sin_yaw,cos_yaw;
    fp32 vx,vy,w;
    sin_yaw=arm_sin_f32(someAngle.gimbalAngleFromChassis);
    cos_yaw=arm_cos_f32(someAngle.gimbalAngleFromChassis);
    w=robotTotalSpeedControl.w;
    if(robotTotalSpeedControl.axis==MovingAxis_e_GimbalAxis)
    {
        //将云台坐标系速度转化为底盘坐标系速度。坐标系变换
        vx=cos_yaw*robotTotalSpeedControl.vx-sin_yaw*robotTotalSpeedControl.vy;
        vy=sin_yaw*robotTotalSpeedControl.vx+cos_yaw*robotTotalSpeedControl.vy;
        
    }
    else if(robotTotalSpeedControl.axis==MovingAxis_e_ChassisAxis)
    {
        vx=robotTotalSpeedControl.vx;
        vy=robotTotalSpeedControl.vy;
    }
    #ifdef INFANTRY
    fp32 vwx = CHASSIS_OMNIWHELL_OMEGA*w;
    fp32 vwy = CHASSIS_OMNIWHELL_OMEGA*w;
    driveMotor[0].wantedMotorSpeed = (vx - vwx) * CHASSIS_OMNIWHELL_SINTHETA - (vy + vwy) * CHASSIS_OMNIWHELL_COSTHETA;
    driveMotor[1].wantedMotorSpeed = (vx + vwx) * CHASSIS_OMNIWHELL_SINTHETA + (vy + vwy) * CHASSIS_OMNIWHELL_COSTHETA;
    driveMotor[2].wantedMotorSpeed = (vx + vwx) * CHASSIS_OMNIWHELL_SINTHETA - (vy - vwy) * CHASSIS_OMNIWHELL_COSTHETA;
    driveMotor[3].wantedMotorSpeed = (vx - vwx) * CHASSIS_OMNIWHELL_SINTHETA + (vy - vwy) * CHASSIS_OMNIWHELL_COSTHETA;
    driveMotor[1].wantedMotorSpeed = -driveMotor[1].wantedMotorSpeed;
    driveMotor[2].wantedMotorSpeed = -driveMotor[2].wantedMotorSpeed;
    #endif
    #ifdef HERO
    driveMotor[0].wantedMotorSpeed=vx-vy-w;    //麦轮速度计算公式
    driveMotor[1].wantedMotorSpeed=-vx-vy-w;
    driveMotor[2].wantedMotorSpeed=-vx+vy-w;
    driveMotor[3].wantedMotorSpeed=vx+vy-w;
    #endif
}


static void firstOrderFilt()
{
    first_order_filter_cali(&robotTotalSpeedControl.vx_filter,robotTotalSpeedControl.vx);
    first_order_filter_cali(&robotTotalSpeedControl.vy_filter,robotTotalSpeedControl.vy);
    first_order_filter_cali(&robotTotalSpeedControl.w_filter,robotTotalSpeedControl.w);
    robotTotalSpeedControl.vx=robotTotalSpeedControl.vx_filter.out;
    robotTotalSpeedControl.vy=robotTotalSpeedControl.vy_filter.out;
    robotTotalSpeedControl.w=robotTotalSpeedControl.w_filter.out;
}

static void calcGiveCurrent(void) //pid 计算电流
{
    uint8_t i;
    for(i=0;i<4;i++)
    {   
        driveMotor[i].presentMotorSpeed=get_chassis_motor_measure_point(i)->speed_rpm*M3508_MOTOR_RPM_TO_VECTOR;    //获取电机转速
        PID_calc(&(driveMotor[i].vpid),driveMotor[i].presentMotorSpeed,driveMotor[i].wantedMotorSpeed);   //pid计算
        driveMotor[i].giveCurrent=driveMotor[i].vpid.out;    //pid的输出就是驱动电流
    }
}




// 2.19，获取功率转折点、抬升电流最大值、转折点电流
void get_power_control_data(uint16_t powerLimit, fp32 *halfPowerLimit, fp32 *liftCurrentMax, fp32 *criticalCurrent)
{
    *halfPowerLimit = (fp32)powerLimit / 3;
    *liftCurrentMax = 0.0f;
    /*
    // 2.19，这些数据还需要进一步实验测
    switch (powerLimit)
    {
        case 40: *liftCurrentMax = 4000.0f; break;
        case 60: *liftCurrentMax = 6000.0f; break;
        case 80: *liftCurrentMax = 8000.0f; break;
        case 100: *liftCurrentMax = 10000.0f; break;
        case 45: *liftCurrentMax = 4500.0f; break;
        case 50: *liftCurrentMax = 5000.0f; break;
        case 55: *liftCurrentMax = 5500.0f; break;
        default: *liftCurrentMax = 0.0f;
    }
    */
    *criticalCurrent = (powerLimit + (FULL_BUFFER - DANGEROUS_BUFFER) / COMMUNICATING_PERIOD_S) / VOLTAGE * CURRENT_TO_DATA;
}


static void powerControl(void)  // 2.3，对计算得到的发送电流进行进一步的功率控制
{
    get_chassis_power_and_buffer(&powerNow, &bufferNow);    // 从裁判系统获取当前功率和缓冲能量

    uint16_t powerLimit;
#ifdef TEST_P40
    powerLimit = 40;
#elif TEST_P60
    powerLimit = 60;
#elif TEST_P80
    powerLimit = 80;
#elif TEST_P100
    powerLimit = 100;
#elif TEST_P45
    powerLimit = 45;
#elif TEST_P50
    powerLimit = 50;
#elif TEST_P55
    powerLimit = 55;
#else
    get_power_limit(&powerLimit);  // 2.19，从裁判系统读取当前功率上限
    powerLimit -= 4;
#endif

    fp32 halfPowerLimit, liftCurrentMax, criticalCurrent;   // 2.19，最大功率上限，功率转折点，抬升电流最大值，转折点电流
    get_power_control_data(powerLimit, &halfPowerLimit, &liftCurrentMax, &criticalCurrent);

    fp32 totalCurrentAfterPID = 0.0f;  // 经过上一步PID计算之后得到的想要发送的底盘总电流
    // 获取经上一步PID计算之后得到的想要发送的底盘总电流
    uint8_t i;
    for (i = 0; i < 4; i++)
    {
        int16_t absCurrent;
        abss(driveMotor[i].giveCurrent, absCurrent);
        totalCurrentAfterPID += absCurrent;
    }

#ifdef NO_BUFFER
    totalCurrentLimit = powerLimit / VOLTAGE * CURRENT_TO_DATA;
    if (powerNow < halfPowerLimit)
        totalCurrentLimit += liftCurrentMax;
    else if (powerNow < powerLimit)
        totalCurrentLimit += (powerLimit - powerNow) * liftCurrentMax / (powerLimit - halfPowerLimit);
#else
    // 下面获取当前状态的最大功率（电流）上限
    // 如果缓冲已经用掉了一部分了
    if (bufferNow < ((FULL_BUFFER) - 0.1f))
    {
        if (bufferNow > DANGEROUS_BUFFER)      // 如果还没有碰到危险线
            totalCurrentLimit = (powerLimit + (bufferNow - DANGEROUS_BUFFER) / COMMUNICATING_PERIOD_S) / VOLTAGE * CURRENT_TO_DATA;
        else    // 如果已经碰到危险线，那就老老实实用最大功率
            totalCurrentLimit = powerLimit / VOLTAGE * CURRENT_TO_DATA;
    }
    // 如果缓冲还是满的，那就说明，还没有达到预计的电流效果，这时电流根本达不到限的值，设定值要足够大，来使得加速过程尽可能快，又不到危险线
    else
    {
        if (powerNow < halfPowerLimit)    // 2.17，如果当前电流没有达到最大合法电流的一半，那就疯转
            totalCurrentLimit = criticalCurrent + liftCurrentMax;
        else    // 2.17，如果超过一半了，稍微限一限，使用定比分点，搞个线性函数限制
            totalCurrentLimit = criticalCurrent + (powerLimit - powerNow) * liftCurrentMax / (powerLimit - halfPowerLimit);
    }
#endif
    
    // 下面根据上限提出的要求按需缩放发送电流
    if (totalCurrentAfterPID > totalCurrentLimit)
    {
        fp32 scale = totalCurrentLimit / totalCurrentAfterPID;
        uint8_t i;
        for (i = 0; i < 4; i++)
            driveMotor[i].giveCurrent *= scale;
    }
}


void chassis_task(void const *pvParameters)
{
    osDelay(CHASSIS_TASK_INIT_TIME);    //等待电机初始化
    initPIDs();                         //初始化PID
    initFilters();
    rc_p=get_remote_control_point();    //获取遥控器数据和NUC数据指针
    nuc_p=get_nuc_control_point();
    robotMode=getRobotPresentMode();

    while(1)
    {
        setChassisMode();
        analyseTotalControl();
        refreshECD();
        firstOrderFilt();
        calcWheelVelocityInChassisAxis();
        calcGiveCurrent();
        #ifdef GAME
        powerControl(); // 2.3，功率控制
        #endif
        if(zeroCurrentMark) 
            CAN_cmd_chassis(0,0,0,0);       //发送零电流
        else
            CAN_cmd_chassis(driveMotor[0].giveCurrent,driveMotor[1].giveCurrent,
                driveMotor[2].giveCurrent,driveMotor[3].giveCurrent);   //正常控制电机
                    osDelay(CHASSIS_CONTROL_TIME_MS);
    }
}

void OLED_chassis(void){
    // OLED_printf(0,0,"zeroCurrentMark: %d", zeroCurrentMark);
    // OLED_printf(0,0,"current:m1:%d,m2:%d,m3:%d,m4:%d",(int32_t)driveMotor[0].giveCurrent,(int32_t)driveMotor[1].giveCurrent)
    // OLED_printf(0,0,"zeroCurrentMark: %d Vx:%.9f,Vy:%.9f,Zw:%.9f Vx:%d,Vy:%d,Zw:%d",zeroCurrentMark,robotTotalSpeedControl.vx,robotTotalSpeedControl.vy,robotTotalSpeedControl.w,nuc_p->x,nuc_p->y,nuc_p->w);
    // OLED_printf(0,0,"AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAVx:%d,Vy:%d,Zw:%d",nuc_p->x,nuc_p->y,nuc_p->w);
    // OLED_printf(0,0,"rc_p->rc.ch[SPINNER_BF_RC]:%d",rc_p->rc.ch[SPINNER_BF_RC]);
    OLED_printf(0,0,"ECD:%d",get_yaw_gimbal_motor_measure_point()->ecd);
}